﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Testcraft.Physics;
using System.Linq;

namespace Testcraft.Scene
{
    class Octree<T> where T : Physics.PhysicsBody
    {
        public class IntersectionInfo
        {
            public T Item { get; set; }
            public BoundingBox Intersection { get; set; }
        }

        public class Cuboid
        {
            public int Depth { get; set; }
            public Cuboid Parent { get; set; }
            public BoundingBox Bounds {get; set;}
            public List<T> Items { get; set; }
            public Cuboid[,,] Children { get; set; }

            public Cuboid()
            {
                this.Parent = null;
                this.Children = null;
                this.Items = new List<T>();
            }
        }

        private const int RECURSIVE_TRESHOLD = 5;
        private int treshold;
        public Cuboid root;

        public List<T> Objects { get; private set; }

        public Octree(int cuboidTreshold)
        {
            this.treshold = cuboidTreshold;
            this.Objects = new List<T>();
            this.root = new Cuboid();
        }

        public void Update()
        {
            for (int i = 0; i < this.Objects.Count; i++)
            {
                if (!this.Objects[i].InRest)
                {
                    List<Cuboid> oldLeafs = new List<Cuboid>();
                    List<Cuboid> newLeafs = new List<Cuboid>();
                    this.FindLeafs(this.root, this.Objects[i].PreviousBounds, oldLeafs);
                    this.FindLeafs(this.root, this.Objects[i].Bounds, newLeafs);

                    for (int j = 0; j < oldLeafs.Count; j++)
                    {
                        oldLeafs[j].Items.Remove(this.Objects[i]);
                    }

                    for (int j = 0; j < newLeafs.Count; j++)
                    {
                        newLeafs[j].Items.Add(this.Objects[i]);
                    }

                    for (int j = 0; j < oldLeafs.Count; j++)
                    {
                        if (oldLeafs[j].Items.Count == 0)
                            this.RemoveLeaf(oldLeafs[j]);
                    }

                    for (int j = 0; j < newLeafs.Count; j++)
                    {
                        if (newLeafs[j].Items.Count > this.treshold && newLeafs[j].Depth < RECURSIVE_TRESHOLD)
                        {
                            Vector3 childrenSize = (newLeafs[j].Bounds.Max - newLeafs[j].Bounds.Min) / 2.0f;
                            newLeafs[j].Children = new Cuboid[2, 2, 2];

                            for (int x = 0; x < 2; x++)
                            {
                                for (int y = 0; y < 2; y++)
                                {
                                    for (int z = 0; z < 2; z++)
                                    {
                                        Cuboid child = new Cuboid();
                                        child.Depth = newLeafs[j].Depth + 1;
                                        child.Parent = newLeafs[j];

                                        Vector3 min = newLeafs[j].Bounds.Min + (childrenSize * new Vector3(x, y, z));
                                        Vector3 max = min + childrenSize;

                                        child.Bounds = new BoundingBox(min, max);

                                        child.Items.AddRange(
                                            newLeafs[j].Items.Where(item => item.Bounds.Intersects(child.Bounds)));

                                        newLeafs[j].Children[x, y, z] = child;

                                        this.BuildTree(child, newLeafs[j].Depth + 1);
                                    }
                                }
                            }
                            newLeafs[j].Items.Clear();
                        }
                    }
                }
            }
        }

        private void RemoveLeaf(Cuboid leaf)
        {
            Cuboid parent = leaf.Parent;
            if (parent != null)
            {
                bool emptyParent = true;
                for (int x = 0; x < 2; x++)
                {
                    for (int y = 0; y < 2; y++)
                    {
                        for (int z = 0; z < 2; z++)
                        {
                            if (!IsCuboidEmpty(parent.Children[x, y, z]))
                                emptyParent = false;
                        }
                    }
                }

                if (emptyParent)
                {
                    // All siblings of leaf are empty. Remove them by
                    // setting parent.Children = null and Parent = null on all siblings.
                    for (int x = 0; x < 2; x++)
                    {
                        for (int y = 0; y < 2; y++)
                        {
                            for (int z = 0; z < 2; z++)
                            {
                                parent.Children[x, y, z].Parent = null;
                            }
                        }
                    }
                    parent.Children = null;
                    RemoveLeaf(parent);
                }
            }
        }

        public bool IsCuboidEmpty(Cuboid cuboid)
        {
            if (cuboid.Items.Count == 0)
            {
                if (cuboid.Children == null)
                {
                    return true;
                }
                else
                {
                    bool areChildrenEmpty = true;
                    for (int x = 0; x < 2; x++)
                    {
                        for (int y = 0; y < 2; y++)
                        {
                            for (int z = 0; z < 2; z++)
                            {
                                if (!IsCuboidEmpty(cuboid.Children[x, y, z]))
                                    areChildrenEmpty = false;
                            }
                        }
                    }

                    return areChildrenEmpty;
                }
            }
            else
            {
                return false;
            }
        }

        private void FindLeafs(Cuboid current, BoundingBox box, List<Cuboid> leafs)
        {
            if (current.Bounds.Intersects(box))
            {
                if (current.Children == null)
                {
                    leafs.Add(current);
                }
                else
                {
                    for (int x = 0; x < 2; x++)
                    {
                        for (int y = 0; y < 2; y++)
                        {
                            for(int z = 0; z < 2; z++)
                            {
                                FindLeafs(current.Children[x, y, z], box, leafs);
                            }
                        }
                    }
                }
            }
        }

        public void Build()
        {
            if(this.Objects.Count == 0)
                return;

            // Calculate maximum bounds
            BoundingBox box = this.Objects[0].Bounds;

            for (int i = 1; i < this.Objects.Count; i++)
            {
                box = BoundingBox.CreateMerged(box, this.Objects[i].Bounds);
            }

            this.root = new Cuboid();
            this.root.Bounds = box;
            this.root.Items = new List<T>(this.Objects);
            this.root.Depth = 1;

            this.BuildTree(this.root, 1);
        }

        private void BuildTree(Cuboid current, int recursionDepth)
        {
            if (current.Items.Count > this.treshold && recursionDepth < RECURSIVE_TRESHOLD)
            {
                Vector3 childrenSize = (current.Bounds.Max - current.Bounds.Min) / 2.0f;
                current.Children = new Cuboid[2,2,2];

                for (int x = 0; x < 2; x++)
                {
                    for (int y = 0; y < 2; y++)
                    {
                        for (int z = 0; z < 2; z++)
                        {
                            Cuboid child = new Cuboid();
                            child.Depth = recursionDepth + 1;
                            child.Parent = current;

                            Vector3 min = current.Bounds.Min + (childrenSize * new Vector3(x, y, z));
                            Vector3 max = min + childrenSize;

                            child.Bounds = new BoundingBox(min, max);

                            child.Items.AddRange(
                                current.Items.Where(item => item.Bounds.Intersects(child.Bounds)));

                            current.Children[x, y, z] = child;

                            this.BuildTree(child, recursionDepth + 1);
                        }
                    }
                }
                current.Items.Clear();
            }
        }

        public List<T> GetIntersections(BoundingBox box)
        {
            List<T> result = new List<T>();
            //if (this.root.Bounds.Intersects(box))
            //{
            //    this.IntersectsInternal2(this.root, box, result);
            //}
            this.IntersectsInternal(this.root, box, result);
            
            return result;
        }

        private void IntersectsInternal2(Cuboid current, BoundingBox box, List<T> result)
        {
            if (current.Children != null)
            {
                Vector3 cuboidCenter = current.Bounds.Min + ((current.Bounds.Max - current.Bounds.Min) / 2.0f);
                for (int x = 0; x < 2; x++)
                {
                    for (int y = 0; y < 2; y++)
                    {
                        for (int z = 0; z < 2; z++)
                        {

                        }
                    }
                }
            }
            else
            {
                result.AddRange(current.Items.Where(child => child.Bounds.Intersects(box)));
            }
        }

        private void IntersectsInternal(Cuboid current, BoundingBox box, List<T> result)
        {
            if (current.Bounds.Intersects(box))
            {
                if (current.Children != null)
                {
                    for (int x = 0; x < 2; x++)
                    {
                        for (int y = 0; y < 2; y++)
                        {
                            for (int z = 0; z < 2; z++)
                            {
                                this.IntersectsInternal(current.Children[x, y, z], box, result);
                            }
                        }
                    }
                }
                else
                {
                    result.AddRange(current.Items.Where(child => child.Bounds.Intersects(box)));
                }
            }
        }

        
    }
}
